Velocity_Kp=-1
Velocity_Ki=0.0
Kd=0

PWM_out=0.0
Encoder_Err=0.0
Encoder_S=0.0
EnC_Err_Lowout=0.0
EnC_Err_Lowout_last=0.0

def pid(encoder_left,encoder_right):
    #global out,Kp,Ki,Kd
    global PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last
    a = 0.7
    Encoder_Err=(encoder_left+encoder_right)-0
    EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last
    EnC_Err_Lowout_last=EnC_Err_Lowout
    Encoder_S+=EnC_Err_Lowout
    #Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S)
    PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S

    
    
    return PWM_out
